#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ipc.h>
#include <sys/types.h>
#include <sys/msg.h>
#include <unistd.h> 

#include <linux/can.h>
#include <linux/can/raw.h>

#include "../app/can_uim.h"
#include "../app/msg_def.h"

// typedef struct _msg_can_frame
// {
//     long type;
//     struct can_frame canFrame;
// }msg_can_frame;


// typedef struct _ctl_uim
// {
//     unsigned char sid;
//     unsigned char opcode;
//     unsigned char dir;
//     unsigned char reserved;
//     unsigned int position;
//     unsigned int queryParam;
// }ctl_uim;


// typedef struct _msg_ctl_uim
// {
//     long type;
//     ctl_uim ctlFrame;
// }msg_ctl_uim;

int recvMsgQueue(int msg_id, int recvType, char out[])
{
    struct msgbuf buf;
    int size=sizeof(buf.mtext);
    if(msgrcv(msg_id, (void*)&buf, size, recvType, 0) < 0)
    {
        perror("msgrcv");
        return -1;
    }

    strncpy(out, buf.mtext, size);
    out[size] = 0;
    return 0;
}


int destoryMsgQueue(int msg_id)
{
    if(msgctl(msg_id, IPC_RMID, NULL) < 0)
    {
        perror("msgctl");
        return -1;
    }
    return 0;
}

int main()
{
    key_t key = ftok(msgQueueKeyPath, 0x1234);
    if(key < 0)
    {
        printf("%s\n",msgQueueKeyPath);
        perror("ftok");        
        return -1;
    }

    int msg_id = msgget(key, IPC_CREAT);
    if(msg_id < 0)
    {
        perror("msgget");
    }


    while(1)
    {
        //recvMsgQueue(msg_id, 1, buf);
        //msg_can_frame msgCanFrame;
        msg_ctl_uim msgCtlUim;
        
        int ret = msgrcv( msg_id, &msgCtlUim, sizeof(ctl_uim) ,1,IPC_NOWAIT );
        //if(ret < 0)
        ///{
        //    perror("msgrcv");
        //    sleep(2);
        //    continue;
        //}
        if(ret>0)
        {
            printf("for uim\n");
            printf("opcode:%d\n",msgCtlUim.ctlFrame.opcode);
            if(msgCtlUim.ctlFrame.opcode == MOVE)
            {
                printf("dir:%d,pos:%d\n",msgCtlUim.ctlFrame.un_cmd.moveCtl.dir,
                msgCtlUim.ctlFrame.un_cmd.moveCtl.position);
            }
        }

        ret = msgrcv( msg_id, &msgCtlUim, sizeof(ctl_uim) ,2,IPC_NOWAIT );
        if(ret>0)
        {
            printf("for sub boards\n");
            printf("opcode:%d\n",msgCtlUim.ctlFrame.opcode);
            if(msgCtlUim.ctlFrame.opcode == MOVE)
            {
                printf("dir:%d,pos:%d\n",msgCtlUim.ctlFrame.un_cmd.moveCtl.dir,
                msgCtlUim.ctlFrame.un_cmd.moveCtl.position);
            }
            else if(msgCtlUim.ctlFrame.opcode == PID_CTL)
            {
                printf("pid opcode:%d\n",msgCtlUim.ctlFrame.un_cmd.pidCtl.pidOpcode);
                if(msgCtlUim.ctlFrame.un_cmd.pidCtl.pidOpcode == 1)
                {
                    printf("kp[%f],ki[%f],kd[%f],tar[%d]\n",
                    msgCtlUim.ctlFrame.un_cmd.pidCtl.kp,
                    msgCtlUim.ctlFrame.un_cmd.pidCtl.ki,
                    msgCtlUim.ctlFrame.un_cmd.pidCtl.kd,
                    msgCtlUim.ctlFrame.un_cmd.pidCtl.target);
                }
                /* code */
            }
            
        }

        //printf("sid:%08x,dlc:%d\n",msgCanFrame.canFrame.can_id,msgCanFrame.canFrame.can_dlc);
        
        sleep(1);
        //if(strcasecmp("quit", buf) == 0)
        //    break;
        //printf("client# %s\n", buf);
    }

    destoryMsgQueue(msg_id);
    return 0;
}


/*#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <stdlib.h>

#define TEXT_SIZE 512

struct _msgbuf
{
    long mtype;
    int status;
    char time[20];

    char mtext[TEXT_SIZE];
};



int main(int argc, char **argv)

{

int msqid;

struct msqid_ds info;

struct _msgbuf buf1;

int flag;

int recvlength;

int key;

int mtype;



key = ftok("msg.tmp", 0x01 ) ;

if ( key < 0 )

{

perror("ftok key error") ;

return -1 ;

}


msqid = msgget( key, 0 ) ;

if ( msqid < 0 )

{

perror("get ipc_id error") ;

return -1 ;

}


recvlength = sizeof(struct msgbuf) - sizeof(long) ;

memset(&buf1, 0x00, sizeof(struct msgbuf)) ;

mtype = 1 ;

flag = msgrcv( msqid, &buf1, recvlength ,mtype,0 ) ;

if ( flag < 0 )

{

perror("recv message error\n") ;

return -1 ;

}

printf("type=%d,time=%s, message=%s\n", buf1.mtype, buf1.time,buf1.mtext);

system("ipcs -q") ;

return 0 ;

}*/